{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from mpl_toolkits import mplot3d\n",
    "# %matplotlib inline\n",
    "%matplotlib notebook\n",
    "import numpy as np\n",
    "import os\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "from IPython.core.display import display, HTML\n",
    "display(HTML(\"<style>.container { width:100% !important; }</style>\"))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "root_dir = '/Users/poodarchu/Desktop/KITTI_teaser'\n",
    "example_id = '007480'\n",
    "raw_lidar = np.fromfile(os.path.join(root_dir, 'velodyne/'+example_id+'.bin'), dtype=np.float32).reshape((-1,4))\n",
    "# raw_lidar = np.fromfile(\"/home/users/benjin.zhu/1542617253237.bin\", dtype=np.float32).reshape(-1, 4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "raw_lidar.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "xdata = raw_lidar[:,0]\n",
    "ydata = raw_lidar[:,1]\n",
    "zdata = raw_lidar[:,2]\n",
    "idata = raw_lidar[:,3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "fig = plt.figure()\n",
    "ax = plt.axes(projection='3d')\n",
    "ax.scatter3D(xdata, ydata, -zdata, c=zdata, marker='.', s=0.1)\n",
    "ax.set_xlabel('X')\n",
    "ax.set_ylabel('Y')\n",
    "ax.set_zlabel('Z')\n",
    "# ax.view_init(,45)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def load_calibration(calib_file):\n",
    "    calib = [x.strip().split() for x in open(calib_file).readlines()]\n",
    "    P0 = np.array([float(i) for i in calib[0][1:]]).reshape((3,4))\n",
    "    P1 = np.array([float(i) for i in calib[1][1:]]).reshape((3,4))\n",
    "    P2 = np.array([float(i) for i in calib[2][1:]]).reshape((3,4))\n",
    "    P3 = np.array([float(i) for i in calib[3][1:]]).reshape((3,4))\n",
    "    R0_rect = np.eye(4, dtype='float32')\n",
    "    R0_3x3 = np.array([float(i) for i in calib[4][1:]]).reshape((3,3))\n",
    "    R0_rect[:3,:3] = R0_3x3\n",
    "    T_v2c = np.eye(4, dtype='float32')\n",
    "    T_v2c[:3,:] = np.array([float(i) for i in calib[5][1:]]).reshape((3,4))\n",
    "    T_vel_to_cam = np.dot(R0_rect, T_v2c)\n",
    "    \n",
    "    calibs = {'P0': P0, 'P1': P1, 'P2': P2,'P3': P3,\n",
    "                'R0_rect': R0_rect,\n",
    "                'T_v2c': T_v2c, 'T_vel_to_cam': T_vel_to_cam}\n",
    "    return calibs\n",
    "\n",
    "# def load_calibration(calib_file):\n",
    "#     calib = [x.strip().split() for x in open(calib_file).readlines()]\n",
    "\n",
    "#     P1 = np.array(list(map(float, calib[0][1:]))).reshape((3, 4))\n",
    "#     R1_3x3 = np.array(list(map(float, calib[1][1:]))).reshape((3, 3))\n",
    "#     R = np.array(list(map(float, calib[2][1:]))).reshape((3, 3))\n",
    "#     t = np.array(list(map(float, calib[3][1:]))).reshape((1, 3))\n",
    "\n",
    "#     R1_rect = np.eye(4, dtype='float32')\n",
    "#     R1_rect[:3, :3] = R1_3x3\n",
    "\n",
    "#     T_v2c = np.eye(4, dtype='float32')\n",
    "#     T_v2c[:3, :3] = R\n",
    "#     T_v2c[:3, 3] = t\n",
    "#     T_vel_to_cam = np.dot(R1_rect, T_v2c)\n",
    "\n",
    "#     calibs = {\n",
    "#         'P1': P1,\n",
    "#         'R1_rect': R1_rect,\n",
    "#         'R': R,\n",
    "#         't': t,\n",
    "#         'T_v2c': T_v2c,\n",
    "#         'T_vel_to_cam': T_vel_to_cam\n",
    "#     }\n",
    "#     return calibs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "calib_file = os.path.join(root_dir, 'calib', example_id+'.txt')\n",
    "calibs = load_calibration(calib_file)\n",
    "type(calibs['P1'])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# def label_to_bbox3d(labels):\n",
    "#     N = len(labels)\n",
    "#     ret = np.zeros((N, 8, 3), dtype=np.float32)\n",
    "    \n",
    "#     for i in range(N):\n",
    "#         box = labels[i]\n",
    "#         translation = box[4:7]\n",
    "#         [l, w, h] = box[1:4]\n",
    "#         rotation = [0, 0, box[-1]]\n",
    "#         trackletBox = np.array([  # in velodyne coordinates around zero point and without orientation yet\n",
    "#             [l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2],\n",
    "#             [w/2, -w/2,-w/2, w/2,  w/2, -w/2,-w/2, w/2],\n",
    "#             [h/2, h/2, h/2,  h/2,  -h/2,-h/2,-h/2, -h/2]\n",
    "#         ])\n",
    "\n",
    "#         # re-create 3D bounding box in velodyne coordinate system\n",
    "#         yaw = rotation[2]\n",
    "                \n",
    "#         rotMat = np.array([\n",
    "#             [np.cos(yaw),  np.sin(yaw), 0], \n",
    "#             [-np.sin(yaw),  np.cos(yaw), 0],\n",
    "#             [0,            0,           1]\n",
    "#         ])\n",
    "        \n",
    "#         cornerPosInVelo = np.dot(trackletBox.T, rotMat) + np.tile(translation, (8, 1))\n",
    "# #         box3d = cornerPosInVelo.transpose()\n",
    "                \n",
    "#         ret[i] = cornerPosInVelo\n",
    "#     return ret\n",
    "\n",
    "def label_to_bbox3d(labels):\n",
    "    N = len(labels)\n",
    "    ret = np.zeros((N, 8, 3), dtype=np.float32)\n",
    "    \n",
    "    for i in range(N):\n",
    "        box = labels[i]\n",
    "        translation = box[4:7]\n",
    "        [h, w, l] = box[1:4]\n",
    "        rotation = [0, 0, box[-1]]\n",
    "        trackletBox = np.array([  # in velodyne coordinates around zero point and without orientation yet\n",
    "            [l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2], \\\n",
    "            [0,0,0,0,-h,-h,-h,-h], \\\n",
    "            [w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2]])\n",
    "\n",
    "        # re-create 3D bounding box in velodyne coordinate system\n",
    "        yaw = rotation[2]\n",
    "        rotMat = np.array([[np.cos(yaw), 0, np.sin(yaw)],\n",
    "                          [0, 1, 0], \n",
    "                          [-np.sin(yaw), 0, np.cos(yaw)]])\n",
    "        cornerPosInVelo = np.dot(rotMat, trackletBox) + \\\n",
    "            np.tile(translation, (8, 1)).T\n",
    "        box3d = cornerPosInVelo.transpose()\n",
    "        ret[i] = box3d\n",
    "    return ret"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "\n",
    "colors = {\n",
    "    'Car': 'b',\n",
    "    'Tram': 'r',\n",
    "    'Cyclist': 'g',\n",
    "    'Van': 'c',\n",
    "    'Truck': 'm',\n",
    "    'Pedestrian': 'y',\n",
    "    'Sitter': 'k'\n",
    "}\n",
    "axes_limits = [\n",
    "    [-70.4, 70.4], # X axis range\n",
    "    [-40, 40], # Y axis range\n",
    "    [-10, 10]   # Z axis range\n",
    "]\n",
    "axes_str = ['X', 'Y', 'Z']\n",
    "\n",
    "def draw_box(pyplot_axis, vertices, axes=[0, 1, 2], color='black'):\n",
    "    \"\"\"\n",
    "    Draws a bounding 3D box in a pyplot axis.\n",
    "    \n",
    "    Parameters\n",
    "    ----------\n",
    "    pyplot_axis : Pyplot axis to draw in.\n",
    "    vertices    : Array 8 box vertices containing x, y, z coordinates.\n",
    "    axes        : Axes to use. Defaults to `[0, 1, 2]`, e.g. x, y and z axes.\n",
    "    color       : Drawing color. Defaults to `black`.\n",
    "    \"\"\"\n",
    "    vertices = np.transpose(vertices)[axes, :]\n",
    "    connections = [\n",
    "        [0, 1], [1, 2], [2, 3], [3, 0],  # Lower plane parallel to Z=0 plane\n",
    "        [4, 5], [5, 6], [6, 7], [7, 4],  # Upper plane parallel to Z=0 plane\n",
    "        [0, 4], [1, 5], [2, 6], [3, 7]  # Connections between upper and lower planes\n",
    "    ]\n",
    "    for connection in connections:\n",
    "        pyplot_axis.plot(*vertices[:, connection], c=color, lw=0.5)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def display_single_lidar(data, boxes=None, points=0.2, view=False):\n",
    "    # points          : Fraction of lidar points to use. Defaults to `0.2`, e.g. 20%.\n",
    "\n",
    "    points_step = int(1. / points)\n",
    "    point_size = 0.01 * (1. / points)\n",
    "    velo_range = range(0, data.shape[0], points_step)\n",
    "    \n",
    "    print(points_step)\n",
    "    print(point_size)\n",
    "    print(velo_range)\n",
    "\n",
    "    def draw_point_cloud(ax, title, axes=[0, 1, 2], xlim3d=None, ylim3d=None, zlim3d=None):\n",
    "        \"\"\"\n",
    "        Convenient method for drawing various point cloud projections as a part of frame statistics.\n",
    "        \"\"\"\n",
    "        ax.scatter(*np.transpose(data[:, axes]), s=point_size, c=data[:, 3], cmap='gray')\n",
    "        \n",
    "        ax.set_title(title)\n",
    "        ax.set_xlabel('{} axis'.format(axes_str[axes[0]]))\n",
    "        ax.set_ylabel('{} axis'.format(axes_str[axes[1]]))\n",
    "        if len(axes) > 2:\n",
    "            ax.set_xlim3d(*axes_limits[axes[0]])\n",
    "            ax.set_ylim3d(*axes_limits[axes[1]])\n",
    "            ax.set_zlim3d(*axes_limits[axes[2]])\n",
    "            ax.set_zlabel('{} axis'.format(axes_str[axes[2]]))\n",
    "        else:\n",
    "            ax.set_xlim(*axes_limits[axes[0]])\n",
    "            ax.set_ylim(*axes_limits[axes[1]])\n",
    "        \n",
    "        # User specified limits\n",
    "        if xlim3d!=None:\n",
    "            ax.set_xlim3d(xlim3d)\n",
    "        if ylim3d!=None:\n",
    "            ax.set_ylim3d(ylim3d)\n",
    "        if zlim3d!=None:\n",
    "            ax.set_zlim3d(zlim3d)\n",
    "        \n",
    "        for i in range(boxes.shape[0]):\n",
    "            draw_box(ax, boxes[i], axes=axes, color='green')\n",
    "            \n",
    "    # Draw point cloud data as 3D plot\n",
    "    f2 = plt.figure(figsize=(10, 5))\n",
    "    ax2 = f2.add_subplot(111, projection='3d')   \n",
    "    ax2.view_init(45,45)\n",
    "    \n",
    "    # Hide grid lines\n",
    "    ax2.grid(False)\n",
    "#     plt.axis('off')\n",
    "    \n",
    "    draw_point_cloud(ax2, 'Velodyne scan', xlim3d=axes_limits[0])\n",
    "    plt.show()\n",
    "    \n",
    "    if view:    \n",
    "        # Draw point cloud data as plane projections\n",
    "        f, ax3 = plt.subplots(3, 1, figsize=(15, 25))\n",
    "#         axe3.view_init(35,100)\n",
    "        draw_point_cloud(\n",
    "            ax3[0], \n",
    "            'Velodyne scan, XZ projection (Y = 0), the car is moving in direction left to right', \n",
    "            axes=[0, 2] # X and Z axes\n",
    "        )\n",
    "        draw_point_cloud(\n",
    "            ax3[1], \n",
    "            'Velodyne scan, XY projection (Z = 0), the car is moving in direction left to right', \n",
    "            axes=[0, 1] # X and Y axes\n",
    "        )\n",
    "        draw_point_cloud(\n",
    "            ax3[2], \n",
    "            'Velodyne scan, YZ projection (X = 0), the car is moving towards the graph plane', \n",
    "            axes=[1, 2] # Y and Z axes\n",
    "        )\n",
    "        plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "labels = os.path.join(root_dir, 'label_2', example_id+'.txt')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "pos_cls = ['Car']\n",
    "ign_cls = ['Van', 'Truck', 'Tram']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def parse_label(labels):\n",
    "    with open(labels, 'r') as fin:\n",
    "        ret = []\n",
    "        for line in fin.readlines():\n",
    "            [cls, truncated, occluded, alpha, lt_x, lt_y, rb_x, rb_y, h, w, l, x, y, z, rotation_y] = line.split(' ')\n",
    "            # if cls in pos_cls: # or  cls in ign_cls\n",
    "            ret.append([cls, float(h), float(w), float(l), float(x), float(y), float(z), float(rotation_y)])\n",
    "        return ret\n",
    "\n",
    "lbl = parse_label(labels)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "bbox3d = label_to_bbox3d(lbl)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(lbl)\n",
    "print(bbox3d.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def project_camera2velo(vel_data_c, calibs):\n",
    "    # vel_data_c: col 0: back -> front\n",
    "    #             col 1: down -> up\n",
    "    #             col 2: left -> right\n",
    "    \n",
    "    vel_data = np.hstack((vel_data_c[:, :3], np.ones((vel_data_c.shape[0], 1), dtype='float32')))\n",
    "    vel_data = np.dot(vel_data, np.linalg.inv(calibs['T_vel_to_cam'].T))\n",
    "    # vel_data /= vel_data[:, -1].reshape((-1, 1))\n",
    "    # vel_data = np.hstack((vel_data[:, :3], vel_data_c[:, -1].reshape((-1, 1))))\n",
    "\n",
    "    return vel_data[:, :3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "bbox3d_lidar = []\n",
    "for box in bbox3d:\n",
    "    box = project_camera2velo(box, calibs)\n",
    "    bbox3d_lidar.append(box)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "bbox3d_lidar = np.array(bbox3d_lidar)\n",
    "bbox3d_lidar.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# def label_to_bbox3d(labels):\n",
    "#     N = len(labels)\n",
    "#     ret = np.zeros((N, 8, 3), dtype=np.float32)\n",
    "    \n",
    "#     for i in range(N):\n",
    "#         box = labels[i]\n",
    "#         translation = box[4:7]\n",
    "#         size = box[1:4]\n",
    "#         rotation = [0, 0, box[-1]]\n",
    "#         h, w, l = size[0], size[1], size[2]\n",
    "#         trackletBox = np.array([  # in velodyne coordinates around zero point and without orientation yet\n",
    "#             [-l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2], \\\n",
    "#             [-w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2], \\\n",
    "#             [0, 0, 0, 0, -h, -h, -h, -h]])\n",
    "\n",
    "#         # re-create 3D bounding box in velodyne coordinate system\n",
    "#         yaw = rotation[2]\n",
    "#         rotMat = np.array([[np.cos(yaw), np.sin(yaw), 0.0],\n",
    "#                            [-np.sin(yaw), np.cos(yaw), 0.0], [0.0, 0.0, 1.0]])\n",
    "#         cornerPosInVelo = np.dot(rotMat, trackletBox) + \\\n",
    "#             np.tile(translation, (8, 1)).T\n",
    "#         box3d = cornerPosInVelo.transpose()\n",
    "#         ret[i] = box3d\n",
    "#     return ret\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# bbox3d = label_to_bbox3d(lbl)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "bbox3d.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "display_single_lidar(raw_lidar, bbox3d_lidar, points=.01, view=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "CORNER2CENTER_AVG = False\n",
    "import math\n",
    "\n",
    "def corner_to_center_box3d(boxes_corner):\n",
    "    # (N, 8, 3) -> (N, 7) x,y,z,h,w,l,ry/z\n",
    "    ret = []\n",
    "    boxes_corner = boxes_corner[:, :, [1, 2, 0]]\n",
    "    for roi in boxes_corner:\n",
    "        if CORNER2CENTER_AVG:  # average version\n",
    "            roi = np.array(roi)\n",
    "            h = abs(np.sum(roi[:4, 1] - roi[4:, 1]) / 4)\n",
    "            w = np.sum(\n",
    "                np.sqrt(np.sum((roi[0, [0, 2]] - roi[3, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[1, [0, 2]] - roi[2, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[4, [0, 2]] - roi[7, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[5, [0, 2]] - roi[6, [0, 2]])**2))) / 4\n",
    "            l = np.sum(\n",
    "                np.sqrt(np.sum((roi[0, [0, 2]] - roi[1, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[2, [0, 2]] - roi[3, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[4, [0, 2]] - roi[5, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[6, [0, 2]] - roi[7, [0, 2]])**2))) / 4\n",
    "            x = np.sum(roi[:, 0], axis=0) / 8\n",
    "            y = np.sum(roi[0:4, 1], axis=0) / 4\n",
    "            z = np.sum(roi[:, 2], axis=0) / 8\n",
    "            ry = np.sum(\n",
    "                math.atan2(roi[2, 0] - roi[1, 0], roi[2, 2] - roi[1, 2]) +\n",
    "                math.atan2(roi[6, 0] - roi[5, 0], roi[6, 2] - roi[5, 2]) +\n",
    "                math.atan2(roi[3, 0] - roi[0, 0], roi[3, 2] - roi[0, 2]) +\n",
    "                math.atan2(roi[7, 0] - roi[4, 0], roi[7, 2] - roi[4, 2]) +\n",
    "                math.atan2(roi[0, 2] - roi[1, 2], roi[1, 0] - roi[0, 0]) +\n",
    "                math.atan2(roi[4, 2] - roi[5, 2], roi[5, 0] - roi[4, 0]) +\n",
    "                math.atan2(roi[3, 2] - roi[2, 2], roi[2, 0] - roi[3, 0]) +\n",
    "                math.atan2(roi[7, 2] - roi[6, 2], roi[6, 0] - roi[7, 0])) / 8\n",
    "            if w > l:\n",
    "                w, l = l, w\n",
    "#                 ry = angle_in_limit(-ry - np.pi / 2) no need to transfer as it in camera coordinate\n",
    "        else:  # max version\n",
    "            h = max(abs(roi[:4, 1] - roi[4:, 1]))\n",
    "            w = np.max(\n",
    "                np.sqrt(np.sum((roi[0, [0, 2]] - roi[3, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[1, [0, 2]] - roi[2, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[4, [0, 2]] - roi[7, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[5, [0, 2]] - roi[6, [0, 2]])**2)))\n",
    "            l = np.max(\n",
    "                np.sqrt(np.sum((roi[0, [0, 2]] - roi[1, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[2, [0, 2]] - roi[3, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[4, [0, 2]] - roi[5, [0, 2]])**2)) +\n",
    "                np.sqrt(np.sum((roi[6, [0, 2]] - roi[7, [0, 2]])**2)))\n",
    "            x = np.sum(roi[:, 0], axis=0) / 8\n",
    "            y = np.sum(roi[0:4, 1], axis=0) / 4\n",
    "            z = np.sum(roi[:, 2], axis=0) / 8\n",
    "            ry = np.sum(\n",
    "                math.atan2(roi[2, 0] - roi[1, 0], roi[2, 2] - roi[1, 2]) +\n",
    "                math.atan2(roi[6, 0] - roi[5, 0], roi[6, 2] - roi[5, 2]) +\n",
    "                math.atan2(roi[3, 0] - roi[0, 0], roi[3, 2] - roi[0, 2]) +\n",
    "                math.atan2(roi[7, 0] - roi[4, 0], roi[7, 2] - roi[4, 2]) +\n",
    "                math.atan2(roi[0, 2] - roi[1, 2], roi[1, 0] - roi[0, 0]) +\n",
    "                math.atan2(roi[4, 2] - roi[5, 2], roi[5, 0] - roi[4, 0]) +\n",
    "                math.atan2(roi[3, 2] - roi[2, 2], roi[2, 0] - roi[3, 0]) +\n",
    "                math.atan2(roi[7, 2] - roi[6, 2], roi[6, 0] - roi[7, 0])) / 8\n",
    "            if w > l:\n",
    "                w, l = l, w\n",
    "\n",
    "\n",
    "#                 ry = angle_in_limit(-ry - np.pi / 2)\n",
    "        ret.append([z, x, y, h, w, l,\n",
    "                    -ry])  # here (0, 1, 2) in camera ---> (2, 0, 1) in lidar\n",
    "\n",
    "    return np.array(ret)\n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "bbox3d_lidar_7 = corner_to_center_box3d(bbox3d_lidar)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "bbox3d_lidar_7"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "[['Car', 1.48, 1.51, 4.35, 0.55, 1.83, 14.93, 1.92],\n",
    " ['Car', 1.47, 1.68, 3.88, 4.24, 1.79, 17.29, 1.94],\n",
    " ['Car', 1.46, 1.74, 3.99, 0.44, 1.67, 9.57, 2.1],\n",
    " ['Car', 1.42, 1.68, 4.29, 2.69, 1.82, 20.21, 1.95],\n",
    " ['Car', 1.36, 1.4, 3.8, 5.6, 1.81, 27.22, 1.96],\n",
    " ['Car', 1.44, 1.65, 2.96, 10.61, 1.89, 39.54, 1.87]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "lbl\\"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
